import math as m
import statistics
import sys
from typing import Optional, Union

import cv2
import numpy as np
import sympy as sp

# from rich import print as rprint
from scipy.linalg import lstsq

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point


def get_mirror_br(course: int, br: int) -> int:
    assert 0 <= course < 360
    assert 0 <= br < 360

    angle_diff = abs(course - br)
    if angle_diff > 180:
        angle_diff = 360 - angle_diff

    br1 = course - angle_diff
    br2 = course + angle_diff

    if br1 < 0:
        br1 = 360 + br1
    elif br1 >= 360:
        br1 = br1 - 360

    if br2 <= 0:
        br2 = 360 + br2
    elif br2 >= 360:
        br2 = br2 - 360

    return br1 if br1 != br else br2


def _cal_try_track_error(track_distance: float, bl: t_lines, cross_line: t_line, bl_time_tick: list[int]) -> float:
    return evaluate_track(track_distance, bl, cross_line, bl_time_tick)[0]


def count_low_points(numbers: list[Union[float, int]]) -> int:
    """given a list of numbers, count how many low points"""
    low_points = 0
    for i in range(1, len(numbers) - 1):
        if numbers[i] < numbers[i - 1] and numbers[i] < numbers[i + 1]:
            low_points += 1
    if low_points > 0:
        if numbers[0] < numbers[1] or numbers[-1] < numbers[-2]:
            low_points += 1
    return low_points


def get_mirror_br(course: int, br: int) -> int:
    assert 0 <= course < 360
    assert 0 <= br < 360

    angle_diff = abs(course - br)
    if angle_diff > 180:
        angle_diff = 360 - angle_diff

    br1 = course - angle_diff
    br2 = course + angle_diff

    if br1 < 0:
        br1 = 360 + br1
    elif br1 >= 360:
        br1 = br1 - 360

    if br2 <= 0:
        br2 = 360 + br2
    elif br2 >= 360:
        br2 = br2 - 360

    return br1 if br1 != br else br2


def moving_average_filter(bearings, window_size=3):
    filtered_bearings = []
    for i in range(len(bearings)):
        start_index = max(0, i - window_size + 1)
        end_index = i + 1
        average = sum(bearings[start_index:end_index]) / (end_index - start_index)
        filtered_bearings.append(average)
    return filtered_bearings


def bearing_change_ratio(bearings):
    ratios = []
    for i in range(1, len(bearings)):
        bearing_change = (bearings[i] - bearings[i - 1] + 180) % 360 - 180
        ratios.append(bearing_change)
    return ratios


def get_angle_between_lines(a1, a2, b1):
    """
    line01 is defined by it's heading a1, a2
    line02 's heading b1
    get the samller angle between them
    """
    res = min(abs(a1 - b1), abs(a2 - b1))
    if res > 90:
        res = 180 - res
    return res


def get_best_adjR(x: list[float], y: list[float], top_degree: int = 8) -> int:
    results = {}
    for degree in range(1, top_degree):
        coeffs = np.polyfit(x, y, degree)
        p = np.poly1d(coeffs)
        yhat = p(x)
        ybar = np.sum(y) / len(y)
        ssreg = np.sum((yhat - ybar) ** 2)
        sstot = np.sum((y - ybar) ** 2)
        results[degree] = 1 - (((1 - (ssreg / sstot)) * (len(y) - 1)) / (len(y) - degree - 1))

    for k, v in results.items():
        if k == 1:
            best_degree = k
            best_v = v
            continue
        if v >= best_v:
            best_degree = k
            best_v = v
        else:
            break

    return best_degree


def get_distance_between_lines(a_line, b_line):
    dist1 = cal_perpendicular_distance(a_line[:2], b_line)
    dist2 = cal_perpendicular_distance(a_line[2:], b_line)
    dist3 = cal_perpendicular_distance(b_line[:2], a_line)
    dist4 = cal_perpendicular_distance(b_line[2:], a_line)
    return max(dist1, dist2, dist3, dist4)


class HoughBundler:
    def __init__(self, min_distance: float = 2.0, min_angle: float = 0.3):
        self.min_distance = min_distance
        self.min_angle = min_angle

    def get_orientation(self, line):
        orientation = m.atan2(abs((line[3] - line[1])), abs((line[2] - line[0])))
        return m.degrees(orientation)

    def check_is_line_different(self, line_1, groups, min_distance_to_merge, min_angle_to_merge):
        for group in groups:
            for line_2 in group:
                if self.get_distance(line_2, line_1) < min_distance_to_merge:
                    orientation_1 = self.get_orientation(line_1)
                    orientation_2 = self.get_orientation(line_2)
                    if abs(orientation_1 - orientation_2) < min_angle_to_merge:
                        group.append(line_1)
                        return False
        return True

    def distance_point_to_line(self, point, line):
        px, py = point
        x1, y1, x2, y2 = line

        def line_magnitude(x1, y1, x2, y2):
            line_magnitude = m.sqrt(m.pow((x2 - x1), 2) + m.pow((y2 - y1), 2))
            return line_magnitude

        lmag = line_magnitude(x1, y1, x2, y2)
        if lmag < 0.00000001:
            distance_point_to_line = 9999
            return distance_point_to_line

        u1 = ((px - x1) * (x2 - x1)) + ((py - y1) * (y2 - y1))
        u = u1 / (lmag * lmag)

        if (u < 0.00001) or (u > 1):
            # // closest point does not fall within the line segment, take the shorter distance
            # // to an endpoint
            ix = line_magnitude(px, py, x1, y1)
            iy = line_magnitude(px, py, x2, y2)
            if ix > iy:
                distance_point_to_line = iy
            else:
                distance_point_to_line = ix
        else:
            # Intersecting point is on the line, use the formula
            ix = x1 + u * (x2 - x1)
            iy = y1 + u * (y2 - y1)
            distance_point_to_line = line_magnitude(px, py, ix, iy)

        return distance_point_to_line

    def get_distance(self, a_line, b_line):
        dist1 = self.distance_point_to_line(a_line[:2], b_line)
        dist2 = self.distance_point_to_line(a_line[2:], b_line)
        dist3 = self.distance_point_to_line(b_line[:2], a_line)
        dist4 = self.distance_point_to_line(b_line[2:], a_line)

        return min(dist1, dist2, dist3, dist4)

    def merge_lines_into_groups(self, lines):
        groups = []  # all lines groups are here
        # first line will create new group every time
        groups.append([lines[0]])
        # if line is different from existing gropus, create a new group
        for line_new in lines[1:]:
            if self.check_is_line_different(line_new, groups, self.min_distance, self.min_angle):
                groups.append([line_new])

        return groups

    def merge_line_segments(self, lines):
        orientation = self.get_orientation(lines[0])

        if len(lines) == 1:
            return np.block([[lines[0][:2], lines[0][2:]]])

        points = []
        for line in lines:
            points.append(line[:2])
            points.append(line[2:])
        if 45 < orientation <= 90:
            # sort by y
            points = sorted(points, key=lambda point: point[1])
        else:
            # sort by x
            points = sorted(points, key=lambda point: point[0])

        return np.block([[points[0], points[-1]]])

    def process_lines(self, lines):
        lines_horizontal = []
        lines_vertical = []

        # for line_i in [l[0] for l in lines]:
        for line_i in lines:
            orientation = self.get_orientation(line_i)
            # if vertical
            if 45 < orientation <= 90:
                lines_vertical.append(line_i)
            else:
                lines_horizontal.append(line_i)

        lines_vertical = sorted(lines_vertical, key=lambda line: line[1])
        lines_horizontal = sorted(lines_horizontal, key=lambda line: line[0])
        merged_lines_all = []

        # for each cluster in vertical and horizantal lines leave only one line
        for i in [lines_horizontal, lines_vertical]:
            if len(i) > 0:
                groups = self.merge_lines_into_groups(i)
                merged_lines = []
                for group in groups:
                    merged_lines.append(self.merge_line_segments(group))
                merged_lines_all.extend(merged_lines)

        data = [list(tuple(i[0][0]) + tuple((i[0][1]))) for i in merged_lines_all]
        return data


def get_vertical_line(line: t_line) -> t_line:
    """return a line vertical to provided line, and pass (x1, y1)"""
    x1, y1, x2, y2 = line
    if x1 == x2:  # Line is vertical
        return [x1, y1, x1 + 100, y1]
    else:
        m = (y2 - y1) / (x2 - x1)  # Calculate the slope of the line
        if m == 0:  # Line is horizontal
            # Return a vertical line passing through the x-coordinate of the line
            return [x1, 0, x1, 100]
        else:
            vertical_slope = -1 / m  # Calculate the slope of the vertical line
            # Choose a point on the original line and extend the vertical line to make it long enough
            x2 = x1 + 100
            y2 = y1 + 100 * vertical_slope
            # Return the vertical line represented by its endpoints
            return [x1, y1, x2, y2]


def get_vertical_line_slope(line: t_line) -> Optional[float]:
    x1, y1, x2, y2 = line
    if x1 == x2:  # Line is vertical
        return None  # Vertical line slope is undefined
    else:
        m = (y2 - y1) / (x2 - x1)  # Calculate the slope of the line
        if m == 0:  # Line is horizontal
            return 0  # Vertical line slope is 0
        else:
            vertical_slope = -1 / m  # Calculate the slope of the vertical line
            return vertical_slope


def if_bl_spread_over_y_axis(lines: t_lines) -> tuple[bool, int]:
    """
    check if bearing lines spread over vertical line
    check if all line slope is positive or negative
    """
    slopes = []
    for line in lines:
        x1, y1, x2, y2 = line
        if x2 == x1:
            slopes.append(sys.maxsize - 10)
        else:
            slopes.append((y2 - y1) / (x2 - x1))

    headings = [max(cal_line_angle(line)) for line in lines]
    if any([i > 360 - s.Vertical_bl_offset for i in headings]) and any([
        i < 180 + s.Vertical_bl_offset for i in headings
    ]):
        cross = True
    else:
        cross = False
    if all(i >= 0 for i in slopes):
        ret = (cross, 1)
    elif all(i <= 0 for i in slopes):
        ret = (cross, -1)
    else:
        ret = (cross, 0)
    return ret


def cal_average_slope(lines: t_lines) -> Union[float, int]:
    slopes = [(y2 - y1) / (x2 - x1) for x1, y1, x2, y2 in lines]
    if if_bl_spread_over_y_axis(lines)[0]:
        positive_slopes = [i for i in slopes if i > 0]
        negative_slopes = [i for i in slopes if i < 0]
        p = sum(positive_slopes) / len(positive_slopes)
        n = sum(negative_slopes) / len(negative_slopes)
        if p >= abs(n):
            average_slope = -(p + n)
        else:
            average_slope = p + abs(n)
    else:
        average_slope = sum(slopes) / len(slopes)

    return average_slope


def calculate_line_intersection_center(lines: t_lines):
    all_points = []
    for i in range(len(lines) - 1):
        for j in range(i + 1, len(lines)):
            x, y = find_intersection_point(lines[i], lines[j])
            all_points.append(sp.Point(x, y, evaluate=False))

    return sp.centroid(*all_points).coordinates


def cal_intersection_center(lines: t_lines, line_format: int = 4) -> tuple[Union[int, float], Union[int, float]]:
    A = np.zeros((len(lines), 3))

    if line_format == 4:
        for i, line in enumerate([(i[0], i[1]), (i[2], i[3])] for i in lines):
            x1, y1 = line[0]
            x2, y2 = line[1]
            A[i, :] = y2 - y1, x1 - x2, x2 * y1 - x1 * y2
    else:
        for i, line in enumerate(lines):
            x1, y1 = line[0]
            x2, y2 = line[1]
            A[i, :] = y2 - y1, x1 - x2, x2 * y1 - x1 * y2

    for index, i in enumerate(A):
        if any(np.isinf(i)) or any(np.isnan(i)):
            A = np.delete(A, index, axis=0)

    X, _, _, _ = lstsq(A[:, :2], -A[:, 2])

    return float(X[0]), float(X[1])


def alter_coordinate_origin(lines: t_lines, image_height: Union[int, float]) -> t_lines:
    """alter coordinate origin and sort point by x-axis
    remove line vertical to x-axis

    Args:
        lines (t_lines):
        height (Union[int,float]):

    Returns:
         t_lines
    """
    result: t_lines = []
    for line in lines:
        x1, y1, x2, y2 = line
        y1 = image_height - y1
        y2 = image_height - y2
        if x1 > x2:
            result.append([x2, y2, x1, y1])
        else:
            result.append([x1, y1, x2, y2])

    return result


def cal_line_angle(line: t_line) -> tuple[float, float]:
    x1, y1, x2, y2 = line
    if (x2 > x1 and y2 > y1) or (x2 < x1 and y2 < y1):
        res = (
            m.atan(abs(x1 - x2) / abs(y1 - y2)),
            m.atan(abs(x1 - x2) / abs(y1 - y2)) + m.pi,
        )
    elif (x2 < x1 and y2 > y1) or (x2 > x1 and y2 < y1):
        res = (
            2 * m.pi - m.atan(abs(x1 - x2) / abs(y1 - y2)),
            m.pi - m.atan(abs(x1 - x2) / abs(y1 - y2)),
        )
    elif x1 == x2:
        res = 0, m.pi
    elif y1 == y2:
        res = 3 * m.pi / 2, m.pi / 2

    return tuple(m.degrees(i) for i in res)


def clean_line_data(lines: t_lines) -> t_lines:
    res: t_lines = []
    lines.sort()
    for line in lines:
        x1, y1, x2, y2 = line
        # !!@ x1== x2 and y1==y2
        if x1 == x2 and y1 == y2:
            continue
        res.append(line)
    return res


def plot_line_through_points(line: t_line = None, p1: t_point = None, p2: t_point = None):
    """plot a line"""
    if line:
        x1, y1, x2, y2 = line
    else:
        x1, y1, x2, y2 = list(p1 + p2)
    x = [x1, x2]
    m = (y2 - y1) / (x2 - x1)
    c = y1 - (m * x1)
    y = [m * xi + c for xi in x]

    ax.plot(x, y)


def find_intersection_point(line_A: t_line, line_B: t_line) -> t_point:
    """get the cross_point of two lines"""
    x1, y1, x2, y2 = line_A
    x3, y3, x4, y4 = line_B

    # Calculate the coefficients for the equations of the lines
    a1 = y2 - y1
    b1 = x1 - x2
    c1 = (y1 * x2) - (y2 * x1)

    a2 = y4 - y3
    b2 = x3 - x4
    c2 = (y3 * x4) - (y4 * x3)

    # Calculate the determinant
    determinant = (a1 * b2) - (a2 * b1)

    # Calculate the cross point coordinates
    if determinant != 0:
        cross_x: float = ((b1 * c2) - (b2 * c1)) / determinant
        cross_y: float = ((a2 * c1) - (a1 * c2)) / determinant
        return [cross_x, cross_y]
    else:
        # @TODO
        raise Exception(f"Iput lines is parallel: ${line_A}, ${line_B}")


def get_line_slope_and_intercept(x1, y1, x2, y2) -> tuple[float, float]:
    """return a line's slope and intercept"""
    slope = (y2 - y1) / (x2 - x1)
    intercept = y1 - slope * x1

    return slope, intercept


def cal_perpendicular_distance(point: t_point, line: t_line):
    """calculate perpendicular distance between a point and a line"""
    x1, y1, x2, y2 = line
    x0, y0 = point
    numerator = abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1))
    denominator = m.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
    distance = numerator / denominator
    return distance


def distance_between_points(p1: t_point, p2: t_point) -> float:
    return m.dist(p1, p2)


def find_vertical_point(line: t_line, p1: t_point) -> tuple[float, float]:
    """
    give a line A and point p1, get the point p2 on line,
    where line(p1, p2) is vertical line A
    """
    x1, y1 = p1
    (x2, y2, x3, y3) = line

    # Calculate the slope of line01
    slope_line01 = (y3 - y2) / (x3 - x2)

    # The slope of the line perpendicular to line01 is the negative reciprocal of the slope of line01
    slope_perpendicular = -1 / slope_line01 if slope_line01 != 0 else float("inf")

    # Using slope and p1 we can find the equation of the perpendicular line that passess through p1
    # Then we find the intersection of this perpendicular line with line01
    if slope_perpendicular == float("inf"):
        x2, y2 = find_intersection_point([x1, y1, x1, y1 + 10], line)
    else:
        # y = m*x + b
        b = y1 - slope_perpendicular * x1

        x2, y2 = find_intersection_point([x1, y1, x1 + 10, (slope_perpendicular * (x1 + 10) + b)], line)

    return (x2, y2)


def find_points_on_line_by_distance(p0: t_point, line: t_line, distance: float | int) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    x0, y0 = p0
    x1, y1, x2, y2 = line

    d_p2l = cal_perpendicular_distance(p0, line)
    if d_p2l > distance:
        return False, (0, 0), (0, 0)

    if x2 == x1:
        # print('x2 = = x1')
        dy = m.sqrt(distance**2 - (x0 - x1) ** 2)
        return True, (x1, y0 + dy), (x1, y0 - dy)

    else:
        x0, y0 = find_vertical_point(line, p0)
        d_shift = m.sqrt(distance**2 - d_p2l**2)
        slope, _ = cal_line_slope_intercept(line)

        # dx ** 2 + (slope * dx) ** 2 = d_shift ** 2
        # dx ** 2 =  d_shift ** 2 / (1 + slope ** 2)
        dx = m.sqrt(d_shift**2 / (1 + slope**2))

        x1 = x0 + dx
        y1 = calculate_y_coordinate(line, x1)
        x2 = x0 - dx
        y2 = calculate_y_coordinate(line, x2)

        assert distance - distance_between_points(p0, [x1, y1]) < 1
        assert distance - distance_between_points(p0, [x2, y2]) < 1
        return True, (x1, y1), (x2, y2)


def divide_track_by_bl_spread(line: t_line, lines_time_tick: list[int]) -> list[t_point]:
    """
    divide track into n segments corresponds to the bearing line spread
    """
    first_idx = lines_time_tick[0]
    lines_time_tick = [i - first_idx for i in lines_time_tick]
    x1, y1, x2, y2 = line
    n = lines_time_tick[-1]
    # print(f"n: {list(range(1,n))}")

    equally_divided_points = [(x1, y1)]
    for i in range(1, n):
        split_point_x = x1 + (i / n) * (x2 - x1)
        split_point_y = y1 + (i / n) * (y2 - y1)
        equally_divided_points.append((split_point_x, split_point_y))

    equally_divided_points.append((x2, y2))
    # print(equally_divided_points)
    # print(len(equally_divided_points))
    return [equally_divided_points[idx] for idx in lines_time_tick]


def cal_line_slope_intercept(line: t_line) -> tuple[float, float]:
    x1, y1, x2, y2 = line
    # Calculate the slope
    slope = (y2 - y1) / (x2 - x1)

    # Calculate the y-intercept
    intercept = y1 - slope * x1

    return slope, intercept


def calculate_y_coordinate(line: t_line, x: Union[int, float], find_x: bool = False) -> float:
    x1, y1, x2, y2 = line

    # Calculate the slope
    slope = (y2 - y1) / (x2 - x1)

    if find_x:
        # Calculate the x-axis value using the point-slope form
        v = x1 + (x - y1) / slope
        return v
    else:
        # Calculate the y-axis value using the point-slope form
        y = y1 + slope * (x - x1)
        return y


class TargetPredction:
    def __init__(self):
        self.track: t_line = []
        self.track_error: float = 9999
        self.track_heading: tuple[float, float] = tuple()
        self.track_distance: float = 0
        self.track_detected: bool = False
        self.top3_track: dict[float, list[float]] = {}


class Target:
    def __init__(
        self,
        head: int = 0,
        tail: int = -1,
        # direction: TgtDirection,
        track_leg: int = 1,
    ) -> None:
        self.head = head
        self.tail = tail
        # self.direction: TgtDirection = direction
        self.track_leg = track_leg

        # self.track_error: float = 0
        # self.best_track: t_line = []
        # self.top3_track: dict[float, list[float]] = {}
        # self.best_track_heading: tuple[float, float] = tuple()
        # self.estimated_travel_distance: float = 0
        # self.track_detected: bool = False
        # self.track_distance_detected: float = travel_distance  # 325.8619339536301

        self.pd_with_track: TargetPredction = TargetPredction()
        self.pd_without_track: TargetPredction = TargetPredction()
        self.ekelund_range: list = []


class TMA:
    def __init__(self, image_file: str, target: Target) -> None:
        # @TODO dynamicly get lines_with_mask
        self.target = target

        self.original_lines: t_lines = []
        self.read_image(image_file)

        self.boundary: t_lines = [self.lines[0], self.lines[-1]]

    def find_target_travel_distance(self, image):
        """
        must be called after self.normalize_line_data()
        estimate target track length
        if traget track detected, return it's lenght
        otherwize, return self.view_center_cross_distance
        """

        # !!@ retrieve #f80101 : akula class
        # !!@ #f80101 is similar to bearing line color, must use following method
        lower_range = np.array([0, 0, 50])
        upper_range = np.array([25, 50, 255])
        mask = cv2.inRange(image, lower_range, upper_range)
        masked_image = cv2.bitwise_and(image, image, mask=mask)

        # cv2.imshow("Extracted Color Range", masked_image)
        # cv2.waitKey(0)
        # cv2.destroyAllWindows()

        gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
        # blur = cv2.GaussianBlur(gray, (5, 5), 0)
        # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

        detected_lines = cv2.HoughLinesP(
            gray,
            1,
            np.pi / s.HoughLinesP_precise_1,
            threshold=s.HoughLinesP_threshold,
            minLineLength=s.HoughLinesP_minLineLength_1,
            maxLineGap=s.HoughLinesP_maxLineGap,
        )

        # !!@ retrieve #00fb00 : virginia class
        if detected_lines is None:
            # Convert the image from BGR to RGB
            image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            # Define the color you want to match (here we use #00fb00)
            target_color = np.array([0, 251, 0])  # BGR values for #00fb00
            # Calculate the color similarity threshold (you can adjust this value)
            color_threshold = 150
            # Calculate the Euclidean distance between each pixel and the target color
            distances = np.linalg.norm(image_rgb - target_color, axis=2)
            # Create a mask based on color similarity threshold
            mask = (distances < color_threshold).astype(np.uint8) * 255
            # Apply the mask to the original image
            masked_image = cv2.bitwise_and(image, image, mask=mask)

            gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
            # blur = cv2.GaussianBlur(gray, (5, 5), 0)
            # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

            # cv2.imshow("Extracted Color Range", masked_image)
            # cv2.waitKey(0)
            # cv2.destroyAllWindows()

            detected_lines = cv2.HoughLinesP(
                gray,
                1,
                np.pi / s.HoughLinesP_precise_1,
                threshold=s.HoughLinesP_threshold,
                minLineLength=s.HoughLinesP_minLineLength_1,
                maxLineGap=s.HoughLinesP_maxLineGap,
            )

        # !!@ retrieve #01ef01 : Los class
        if detected_lines is None:
            # Convert BGR to HSV
            hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
            # #01ef01
            # Define the lower and upper bounds of the green color in HSV
            lower_green = np.array([40, 50, 50])
            upper_green = np.array([80, 255, 255])
            # Create a mask to keep only green colors
            mask = cv2.inRange(hsv_image, lower_green, upper_green)
            # gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
            # blur = cv2.GaussianBlur(gray, (5, 5), 0)
            # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

            # cv2.imshow("Extracted Color Range", mask)
            # cv2.waitKey(0)
            # cv2.destroyAllWindows()

            detected_lines = cv2.HoughLinesP(
                mask,
                1,
                np.pi / s.HoughLinesP_precise_1,
                threshold=s.HoughLinesP_threshold,
                minLineLength=s.HoughLinesP_minLineLength_1,
                maxLineGap=s.HoughLinesP_maxLineGap,
            )

        lines: t_lines = []

        # print("1" * 111)
        # print(detected_lines)
        # print("2" * 111)

        if detected_lines is not None:
            for line in detected_lines:
                lines.append(list(line[0]))
            self.target.pd_with_track.track_distance = (
                max([distance_between_points(line[:2], line[2:]) for line in lines]) - s.Target_track_arrow
            )
            # print(detected_lines)
            # print("1" * 11)
            # print(lines)
            # print(self.target.track)
            # print(self.normalized_lines)
            # sys.exit(1)
            self.target.pd_with_track.track_detected = True
        else:
            self.target.pd_without_track.track_distance = self.view_center_cross_distance

    def normalize_line_data(self):
        """replace every line with intersection_point of image boundary
        sorted lines
        merged similar lines
        """
        # y = m*x + b
        unsort_lines: t_lines = []

        self.average_slope = cal_average_slope(self.switch_coordinate_lines)
        # !!@ extend all lines to reach image-box
        for idx, line in enumerate(self.switch_coordinate_lines):
            x1, y1, x2, y2 = line
            x1, y1, x2, y2 = float(x1), float(y1), float(x2), float(y2)
            unsort_lines.append([])
            if x1 == x2:
                unsort_lines[idx] = [x1, 0, x1, self.canvas_size[0]]
                continue
            if y1 == y2:
                unsort_lines[idx] = [0, y1, self.canvas_size[1], y1]
                continue
            for box_line in self.image_box:
                x, y = find_intersection_point(line, box_line)
                if x < 0 or x > self.canvas_size[1] or y < 0 or y > self.canvas_size[0]:
                    continue
                unsort_lines[idx].extend([x, y])
            assert len(unsort_lines[idx]) == 4

        all_boundary_points = []
        for line in unsort_lines:
            all_boundary_points.append(sp.Point(line[:2], evaluate=False))
            all_boundary_points.append(sp.Point(line[2:], evaluate=False))

        self.view_center: t_point = sp.centroid(*all_boundary_points).coordinates

        # @TODO no more bl_intersection_center
        self.bl_intersection_center = cal_intersection_center(unsort_lines)

        # @TODO cornor case: slope == 0, group_cross_line is vertical to x-axis
        slope = -1 / self.average_slope
        # y = m*x + b
        x_vc, y_vc = self.view_center
        group_cross_line = [x_vc, y_vc, x_vc + 50, y_vc + slope * 50]
        self.view_center_group_cross_line = group_cross_line

        # !!@ sort lines
        # some lines are very close to each other, them may cross !!!
        cross_points = [(find_intersection_point(group_cross_line, i), index) for index, i in enumerate(unsort_lines)]
        ordered_index = [i[1] for i in sorted(cross_points, key=lambda item: item[0][0])]
        ordered_lines = [unsort_lines[i] for i in ordered_index]
        self.normalized_lines = ordered_lines
        self.view_center_cross_distance = distance_between_points(
            find_intersection_point(ordered_lines[0], group_cross_line),
            find_intersection_point(ordered_lines[-1], group_cross_line),
        )

        # !!@ merge similar lines
        line_merger = HoughBundler(2, min_angle=0.4)
        self.lines = line_merger.process_lines(self.normalized_lines)
        self.lines = line_merger.process_lines(self.lines)

        # !!@ sort merged lines
        # after this, we can guarantee all merged lines are sorted
        cross_points = [(find_intersection_point(group_cross_line, i), index) for index, i in enumerate(self.lines)]
        ordered_index = [i[1] for i in sorted(cross_points, key=lambda item: item[0][0])]
        ordered_lines = [self.lines[i] for i in ordered_index]

        # !!@ resolve a corner case for HoughBundler
        # (line1: 45.2 line2: 49.2 will be assigned to different groups)
        final_merge_lines = []
        for index, line in enumerate(ordered_lines):
            if index == 0:
                final_merge_lines.append(line)
                continue
            if get_distance_between_lines(ordered_lines[index], final_merge_lines[-1]) < 5:
                pre_line = final_merge_lines[-1]
                new_line = [
                    (pre_line[0] + line[0]) / 2,
                    (pre_line[1] + line[1]) / 2,
                    (pre_line[2] + line[2]) / 2,
                    (pre_line[3] + line[3]) / 2,
                ]
                if distance_between_points(new_line[:2], new_line[2:]) < 30:
                    final_merge_lines[-1] = [
                        (pre_line[0] + line[2]) / 2,
                        (pre_line[1] + line[3]) / 2,
                        (pre_line[2] + line[0]) / 2,
                        (pre_line[3] + line[1]) / 2,
                    ]
                else:
                    final_merge_lines[-1] = new_line
                t = final_merge_lines[-1]
                assert distance_between_points(t[:2], t[2:]) > 30
            else:
                final_merge_lines.append(line)

        # !!@ ensure line slope of self.lines[0] > [-1]
        if abs(get_line_slope_and_intercept(*final_merge_lines[0])[0]) > abs(
            get_line_slope_and_intercept(*final_merge_lines[-1])[0]
        ):
            final_merge_lines.reverse()
        self.lines = final_merge_lines
        self.lines_time_tick = list(range(len(final_merge_lines)))

        # !!@ image size used for echart x-axis/y-axis
        max_x = 0
        max_y = 0
        min_x = 1000
        min_y = 1000
        for line in self.lines:
            if max_x < max(line[0], line[2]):
                max_x = max(line[0], line[2])
            if max_y < max(line[1], line[3]):
                max_y = max(line[1], line[3])
            if min_x > min(line[0], line[2]):
                min_x = min(line[0], line[2])
            if min_y > min(line[1], line[3]):
                min_y = min(line[1], line[3])
        self.image_size = [min_x, min_y, max_x, max_y]

    def single_leg_ekelund_ranging(self):
        self.target.ekelund_range = []
        self.bl_feature = if_bl_spread_over_y_axis(self.lines)

        self.bl_headings = [sorted(cal_line_angle(line)) for line in self.lines]
        slopes = []
        for line in self.lines:
            x1, y1, x2, y2 = line
            if x2 == x1:
                slopes.append(sys.maxsize - 10)
            else:
                slopes.append((y2 - y1) / (x2 - x1))
        self.bl_slopes = slopes

        if self.bl_feature[1] != 0:
            bl_orientation = [min(i) for i in self.bl_headings]
        else:
            if self.bl_feature[0]:
                # print("bearing lines spread over y-axis")
                bl_orientation = []
                for index, hd in enumerate(self.bl_headings):
                    if self.bl_slopes[index] >= 0:
                        bl_orientation.append(max(hd))
                    else:
                        bl_orientation.append(min(hd))
            else:
                # print("bearing lines spread over x-axis")
                bl_orientation = [min(i) for i in self.bl_headings]
        self.bl_orientation = bl_orientation

        self.calculate_bl_change_ratio_polyfit()

        assert len(self.bl_change_ratio) == len(self.lines)

        for idx, ratio in enumerate(self.bl_change_ratio):
            self.target.ekelund_range.append({
                j: round(abs(j / ratio), 1)
                for j in range(
                    s.Ekelund_range_spd_btm,
                    s.Ekelund_range_spd_top,
                    s.Ekelund_range_spd_step,
                )
            })

    def calculate_bl_change_ratio_raw(self):
        """
        use np.poly1d/np.polyfit to adjust target bearing line change ratio
        self.bl_change_ratio
        """
        bl_change_ratio_raw = []
        bl_orientation = self.bl_orientation
        for idx in range(1, len(bl_orientation)):
            v = (bl_orientation[idx] - bl_orientation[idx - 1]) / (
                self.lines_time_tick[idx] - self.lines_time_tick[idx - 1]
            )
            bl_change_ratio_raw.append(v)
        self.bl_change_ratio_raw = bl_change_ratio_raw
        assert len(self.bl_change_ratio_raw) == len(self.lines) - 1
        if len(self.lines) <= 3:
            self.bl_change_ratio_raw.insert(
                0,
                (bl_orientation[0] - bl_orientation[1]) / (self.lines_time_tick[0] - self.lines_time_tick[1]),
            )
            self.bl_change_ratio = self.bl_change_ratio_raw
            return

        x_data = [float(i) for i, _ in enumerate(self.bl_change_ratio_raw)]
        y_data = self.bl_change_ratio_raw[::]
        best_dgr = get_best_adjR(x_data, y_data)
        best_fit_fn = np.poly1d(np.polyfit(x_data, y_data, best_dgr))

        self.bl_change_ratio = [float(best_fit_fn(i)) for i in range(-1, len(self.bl_change_ratio_raw))]

    def calculate_bl_change_ratio_polyfit(self):
        """
        use np.poly1d/np.polyfit to adjust target bearing line change ratio
        self.bl_change_ratio
        """
        bl_change_ratio_raw = []
        bl_orientation = self.bl_orientation
        for idx in range(1, len(bl_orientation) - 1):
            v = (bl_orientation[idx + 1] - bl_orientation[idx - 1]) / (
                self.lines_time_tick[idx + 1] - self.lines_time_tick[idx - 1]
            )
            bl_change_ratio_raw.append(v)
        self.bl_change_ratio_raw = bl_change_ratio_raw
        assert len(self.bl_change_ratio_raw) == len(self.lines) - 2
        if len(self.lines) <= 3:
            self.bl_change_ratio_raw.append(
                (bl_orientation[-1] - bl_orientation[-2]) / (self.lines_time_tick[-1] - self.lines_time_tick[-2])
            )

            self.bl_change_ratio_raw.insert(
                0,
                (bl_orientation[0] - bl_orientation[1]) / (self.lines_time_tick[0] - self.lines_time_tick[1]),
            )
            self.bl_change_ratio = self.bl_change_ratio_raw
            return

        x_data = [float(i) for i, _ in enumerate(self.bl_change_ratio_raw)]
        y_data = self.bl_change_ratio_raw[::]
        best_dgr = get_best_adjR(x_data, y_data)
        best_fit_fn = np.poly1d(np.polyfit(x_data, y_data, best_dgr))

        self.bl_change_ratio = [float(best_fit_fn(i)) for i in range(-1, len(self.bl_change_ratio_raw) + 1)]

    def adjust_all_target_bearing(self):
        """
        use np.poly1d/np.polyfit to adjust target bearing
        self.bl_orientation_adjusted
        """
        y_data = self.bl_orientation
        x_data = [float(i) for i in range(len(y_data))]
        best_dgr = get_best_adjR(x_data, y_data)
        best_fit_fn = np.poly1d(np.polyfit(x_data, y_data, best_dgr))

        polyline = np.linspace(0, len(x_data) - 1, len(x_data))
        self.bl_orientation_adjusted = [float(i) for i in best_fit_fn(polyline)]

    def read_image(self, image_file: str) -> tuple[t_lines, t_lines, t_lines, t_point]:
        image = cv2.imread(image_file)

        # !!@ mask left-up cornor
        mask_area = (
            0,
            1,
            m.ceil(s.Image_maskarea_ratio.w * image.shape[1]),
            m.ceil(s.Image_maskarea_ratio.h * image.shape[0]),
        )
        # rectangle coordinates (x, y, width, height)
        x, y, w, h = mask_area
        image[y : y + h, x : x + w] = [0, 0, 0]

        image_copy = image.copy()

        # @TODO restrict image.shape (500, 630, 3)

        # (height, width)
        self.canvas_size = (image.shape[0], image.shape[1])
        self.image_box = (
            (0, 0, 0, self.canvas_size[0]),
            (0, self.canvas_size[0], self.canvas_size[1], self.canvas_size[0]),
            (self.canvas_size[1], self.canvas_size[0], self.canvas_size[1], 0),
            (self.canvas_size[1], 0, 0, 0),
        )

        # !!@ method 1 retrieve bearing lines
        # image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # lower_range = np.array([56, 0, 92])
        # upper_range = np.array([179, 255, 255])
        # masked_image = cv2.inRange(image, lower_range, upper_range)

        # !!@ method 2 retrieve bearing lines
        image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        # Define the color you want to match (here we use #b300b3)
        target_color = np.array([179, 0, 179])  # BGR values for #b300b3
        # Calculate the color similarity threshold (you can adjust this value)
        color_threshold = 150
        # Calculate the Euclidean distance between each pixel and the target color
        distances = np.linalg.norm(image_rgb - target_color, axis=2)
        # Create a mask based on color similarity threshold
        mask = (distances < color_threshold).astype(np.uint8) * 255
        # Apply the mask to the original image
        masked_image = cv2.bitwise_and(image, image, mask=mask)

        gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
        # blur = cv2.GaussianBlur(gray, (5, 5), 0)
        # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

        original_detected_lines = cv2.HoughLinesP(
            gray,  # Input edge image
            1,  # Distance resolution in pixels
            np.pi / s.HoughLinesP_precise_2,  # Angle resolution in radians
            threshold=s.HoughLinesP_threshold,  # Min number of votes for valid line
            minLineLength=s.HoughLinesP_minLineLength_2,  # Min allowed length of line
            maxLineGap=s.HoughLinesP_maxLineGap,  # Max allowed gap between line for joining them
        )

        # print(original_detected_lines)
        # for index, line in enumerate(original_detected_lines):
        #     x1, y1, x2, y2 = line[0]
        #     cv2.line(masked_image, (x1, y1), (x2, y2), (255, 255, 255), 1)
        # cv2.imshow("Detected Lines", masked_image)
        # cv2.waitKey(0)
        # cv2.destroyAllWindows()
        # raise Exception('231')

        # Iterate through the detected lines and draw them on the original image:
        if original_detected_lines is None:
            # @TODO handle exception
            raise Exception("No bearing line detected in image")

        for line in original_detected_lines:
            self.original_lines.append(list(line[0]))

        self.switch_coordinate_lines = alter_coordinate_origin(self.original_lines, image.shape[0])
        self.normalize_line_data()
        self.find_target_travel_distance(image_copy)
        # @TODO mask line
        self.lines_with_mask = self.lines
        # display lines
        # @TODO display all lines properly
        self.display_lines: t_lines = self.lines
        # for line in self.lines:
        #     x1, y1, x2, y2 = line
        #     if x1 < 0:
        #         x1 = x2
        #         y1 = y2
        #         x2 = self.canvas_size[1]
        #         y2 = calculate_y_coordinate(line, x2)
        #     if y1 < 0:
        #         x1 = x2
        #         y1 = y2
        #         y2 = self.canvas_size[0]
        #         x2 = calculate_y_coordinate(line, y2, find_x=True)
        #     self.display_lines.append([x1, y1, x2, y2])

    def predict_with_specified_target_track(self, travel_distance: float = 0) -> tuple[float]:
        assert travel_distance > 0
        lines_used = self.lines
        track_fitness_error_collection = {}

        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")
        try_point = outer_lines_cross_p
        # point on x-axis where line pass it and view_center is vertical to first_line
        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        smallest_error = sys.maxsize
        while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance:
            try_point = (
                try_point[0] + try_step,
                calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
            )
            ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance)
            if not ok:
                break
            p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
            p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
            if min(p1_error, p2_error) > smallest_error:
                break

            if p1_error <= p2_error:
                track_fitness_error_collection[p1_error] = (try_point, p1)
            else:
                track_fitness_error_collection[p2_error] = (try_point, p2)

            smallest_error = min(p1_error, p2_error, smallest_error)

        if not track_fitness_error_collection:
            raise Exception("track_fitness_error comparetion error!")

        (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
        return (x1, y1, x2, y2, smallest_error)

    def predict_with_target_track(self):
        if not self.target.pd_with_track.track_detected:
            return

        lines_used = self.lines
        track_fitness_error_collection = {}
        travel_distance_used = self.target.pd_with_track.track_distance
        assert travel_distance_used > 0

        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")
        try_point = outer_lines_cross_p
        # point on x-axis where line pass it and view_center is vertical to first_line
        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        smallest_error = sys.maxsize
        while cal_perpendicular_distance(try_point, lines_used[-1]) <= self.target.pd_with_track.track_distance:
            try_point = (
                try_point[0] + try_step,
                calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
            )
            ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
            print("-" * 88)
            print(self.lines)
            if not ok:
                break
            p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
            p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
            if min(p1_error, p2_error) > smallest_error * 15:
                break

            if p1_error <= p2_error:
                track_fitness_error_collection[p1_error] = (try_point, p1)
            else:
                track_fitness_error_collection[p2_error] = (try_point, p2)

            smallest_error = min(p1_error, p2_error, smallest_error)

        if not track_fitness_error_collection:
            raise Exception("track_fitness_error comparetion error!")

        (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
        # self.target.best_track = ((x1, y1), (x2, y2))
        self.target.pd_with_track.track_error = 10 * smallest_error / len(self.lines)
        self.target.pd_with_track.track = [x1, y1, x2, y2]
        self.target.pd_with_track.track_heading = cal_line_angle([x1, y1, x2, y2])
        self.target.pd_with_track.track_distance = travel_distance_used

    def predict_without_target_track_full_brute_force(self, btm_limit: float = 0, top_limit: float = 0):
        travel_distance_step = s.Travel_distance_step

        self.target.pd_without_track.track_error = 9999
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min
            top_limit = s.Travel_distance_max

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        for distance in range(int(btm_limit), int(top_limit), travel_distance_step):
            try_point = outer_lines_cross_p
            track_fitness_error_collection = {}
            travel_distance_used = distance

            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
                p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
                if p1_error <= p2_error:
                    track_fitness_error_collection[p1_error] = (try_point, p1)
                else:
                    track_fitness_error_collection[p2_error] = (try_point, p2)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not track_fitness_error_collection:
                continue

            # if 410 < travel_distance_used < 420:
            # print(f"{travel_distance_used} - {smallest_error}")

            (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]

            if (
                self.target.pd_without_track.track_error == 0
                or self.target.pd_without_track.track_error > smallest_error
            ):
                self.target.pd_without_track.track_error = smallest_error
                self.target.pd_without_track.track = [x1, y1, x2, y2]
                self.target.pd_without_track.track_heading = cal_line_angle([
                    x1,
                    y1,
                    x2,
                    y2,
                ])
                self.target.pd_without_track.track_distance = travel_distance_used

        self.target.pd_without_track.track_error = 10 * self.target.pd_without_track.track_error / len(self.lines)

    def predict_without_target_track_partial_brute_force(self, btm_limit: float = 0, top_limit: float = 0):
        error_limit = 500

        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min
            top_limit = s.Travel_distance_max

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        distance_fit_error_collection = {}
        for travel_distance_used in range(int(btm_limit), int(top_limit), s.Travel_distance_step):
            try_point = outer_lines_cross_p

            track_fitness_error_collection = {}
            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
                p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
                if min(p1_error, p2_error) > smallest_error * 15:
                    break
                if p1_error <= p2_error:
                    track_fitness_error_collection[p1_error] = (try_point, p1)
                else:
                    track_fitness_error_collection[p2_error] = (try_point, p2)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not track_fitness_error_collection:
                continue

            if (
                len(distance_fit_error_collection) > 1
                and smallest_error > min(distance_fit_error_collection.keys()) * error_limit
            ):
                break

            (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
            distance_fit_error_collection[smallest_error] = [
                [x1, y1, x2, y2],
                travel_distance_used,
            ]

        smallest_error = min(distance_fit_error_collection.keys())
        best_distance = distance_fit_error_collection[smallest_error][1]
        x1, y1, x2, y2 = distance_fit_error_collection[smallest_error][0]
        self.target.pd_without_track.track = [x1, y1, x2, y2]
        self.target.pd_without_track.track_heading = cal_line_angle([
            x1,
            y1,
            x2,
            y2,
        ])
        self.target.pd_without_track.track_distance = best_distance
        self.target.pd_without_track.track_error = 10 * smallest_error / len(self.lines)

    def predict_without_target_track(self):
        self.target.pd_without_track.track_error = 9999
        # lines_used = self.normalized_lines
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])

        track1 = s.Travel_distance_step
        track4 = s.Travel_distance_max

        while track1 + 3 < track4:
            track2 = (track4 - track1) / 3 + track1
            track3 = (track4 - track1) * 2 / 3 + track1
            errors = []
            for travel_distance_used in [track1, track2, track3, track4]:
                try_point = outer_lines_cross_p
                track_fitness_error_collection = {}

                if not try_point:
                    raise Exception("First and last bl seems parallel")

                x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

                if x_value < outer_lines_cross_p[0]:
                    try_step = -s.Try_point_step
                elif x_value > outer_lines_cross_p[0]:
                    try_step = s.Try_point_step
                else:
                    # @TODO first_line is vertical to x-axis
                    raise Exception("predict_target_track error: first_line is vertical to x-axis")

                while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                    try_point = (
                        try_point[0] + try_step,
                        calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                    )
                    ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                    if not ok:
                        break
                    p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
                    p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
                    if p1_error <= p2_error:
                        track_fitness_error_collection[p1_error] = (
                            try_point,
                            p1,
                        )
                    else:
                        track_fitness_error_collection[p2_error] = (
                            try_point,
                            p2,
                        )

                if not track_fitness_error_collection:
                    errors.append(9999)
                    continue

                smallest_error = min(track_fitness_error_collection.keys())

                errors.append(smallest_error)

                (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]

                if (
                    self.target.pd_without_track.track_error == 0
                    or self.target.pd_without_track.track_error > smallest_error
                ):
                    self.target.pd_without_track.track_error = smallest_error
                    # self.target.best_track = ((x1, y1), (x2, y2))
                    self.target.pd_without_track.track = [x1, y1, x2, y2]
                    self.target.pd_without_track.track_heading = cal_line_angle([
                        x1,
                        y1,
                        x2,
                        y2,
                    ])
                    self.target.pd_without_track.track_distance = travel_distance_used

            assert len(errors) == 4
            smallest_error = min(errors)
            count = count_low_points(errors)
            if smallest_error == errors[0] and count == 0:
                track4 = track3
            elif smallest_error == errors[1] and count == 1:
                track4 = track3
            elif smallest_error == errors[2] and count == 1:
                track1 = track2
            elif smallest_error == errors[3] and count == 0:
                track1 = track2
            else:
                stdev = statistics.stdev([track1, track2, track3, track4])
                if stdev > 20:
                    print("1" * 88)
                    print("Multi low points situation,  switch to brute force method")
                    print(track1, track2, track3, track4)
                    print(errors)
                    print(statistics.stdev([track1, track2, track3, track4]))
                    print(([track1, track4]))
                    self.predict_without_target_track_partial_brute_force(
                        btm_limit=track1,
                        top_limit=track4,
                    )
                    break
                else:
                    print("2" * 88)
                    print("go for average track")
                    print(track1, track2, track3, track4)
                    print(errors)
                    print(statistics.stdev([track1, track2, track3, track4]))
                    print(([track1, track4]))
                    tracks = []
                    for track in range(m.floor(track1), m.ceil(track4), 1):
                        tracks.append(self.predict_with_specified_target_track(track))

                    average_value = list(sum(x) / len(tracks) for x in zip(*tracks))
                    # # self.target.best_track = ((x1, y1), (x2, y2))
                    # self.target.pd_without_track.track = [x1, y1, x2, y2]
                    # self.target.pd_without_track.track_heading = cal_line_angle([
                    #     x1,
                    #     y1,
                    #     x2,
                    #     y2,
                    # ])
                    self.target.pd_without_track.track_error = average_value[-1]
                    self.target.pd_without_track.track_heading = cal_line_angle(average_value[:4])
                    self.target.pd_without_track.track_distance = (track1 + track4) / 2
                    self.target.pd_without_track.track = average_value[:4]
                    break
        else:
            self.target.pd_without_track.track_error = 10 * self.target.pd_without_track.track_error / len(self.lines)

        if self.target.pd_without_track.track_distance >= s.Travel_distance_max - 5:
            self.predict_without_target_track_partial_brute_force(
                btm_limit=self.target.pd_without_track.track_distance,
                top_limit=20000,
            )

    def predict_without_target_track_new_02(self, btm_limit: float = 0, top_limit: float = 0):
        error_limit = 500

        self.target.pd_without_track.track_error = 9999
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step_first
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step_first
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        distance_fit_error_collection = {}
        for travel_distance_used in range(int(btm_limit), int(top_limit), s.Travel_distance_step_first):
            try_point = outer_lines_cross_p
            try_point_error_collection = []
            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
                p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
                if min(p1_error, p2_error) > smallest_error * 15:
                    break
                if p1_error <= p2_error:
                    try_point_error_collection.append(p1_error)
                else:
                    try_point_error_collection.append(p2_error)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not try_point_error_collection:
                continue

            if (
                len(distance_fit_error_collection) > 1
                and smallest_error > min(distance_fit_error_collection.keys()) * error_limit
            ):
                break

            distance_fit_error_collection[smallest_error] = travel_distance_used

        first_stage_smallest_error = min(distance_fit_error_collection.keys())
        first_stage_best_distance = distance_fit_error_collection[first_stage_smallest_error]
        # print(first_stage_best_distance, first_stage_smallest_error)
        self.predict_without_target_track_partial_brute_force(
            first_stage_best_distance - 3 * s.Travel_distance_step_first,
            first_stage_best_distance + 3 * s.Travel_distance_step_first,
        )

    def predict_without_target_track_new(self, btm_limit: float = 0, top_limit: float = 0):
        error_limit = 500

        self.target.pd_without_track.track_error = 9999
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step_first
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step_first
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        distance_fit_error_collection = {}
        for travel_distance_used in range(int(btm_limit), int(top_limit), s.Travel_distance_step_first):
            try_point = outer_lines_cross_p
            try_point_error_collection = []
            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = self.cal_track_fitness(try_point, p1, lines_used, self.lines_time_tick)
                p2_error = self.cal_track_fitness(try_point, p2, lines_used, self.lines_time_tick)
                if min(p1_error, p2_error) > smallest_error * 15:
                    break
                if p1_error <= p2_error:
                    try_point_error_collection.append(p1_error)
                else:
                    try_point_error_collection.append(p2_error)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not try_point_error_collection:
                continue

            if (
                len(distance_fit_error_collection) > 1
                and smallest_error > min(distance_fit_error_collection.keys()) * error_limit
            ):
                break

            distance_fit_error_collection[smallest_error] = travel_distance_used

        first_stage_smallest_error = min(distance_fit_error_collection.keys())
        first_stage_best_distance = distance_fit_error_collection[first_stage_smallest_error]
        print(first_stage_best_distance, first_stage_smallest_error)
        self.predict_without_target_track_partial_brute_force(
            first_stage_best_distance - 3 * s.Travel_distance_step_first,
            first_stage_best_distance + 3 * s.Travel_distance_step_first,
        )

    @staticmethod
    def cal_track_fitness(
        head_point: t_point,
        tail_point: t_point,
        lines_used: t_lines,
        lines_time_tick: list[int],
    ) -> float:
        """evaluate track fitness"""
        assume_track_line = list(head_point + tail_point)
        error_list = []
        points = divide_track_by_bl_spread(assume_track_line, lines_time_tick)
        assert len(points) == len(lines_used)
        for point, line in zip(points, lines_used):
            error_list.append(distance_between_points(point, find_intersection_point(assume_track_line, line)))
        y_true = [0 for _ in range(len(lines_used))]
        y_pred = error_list
        distance = distance_between_points(head_point, tail_point)
        return sum((yt - yp) ** 2 for yt, yp in zip(y_true, y_pred)) / (len(y_true) * distance)

    @staticmethod
    def compare_distance_from_point(point1, point2, fixed_point):
        distance_to_point1 = m.sqrt((point1[0] - fixed_point[0]) ** 2 + (point1[1] - fixed_point[1]) ** 2)
        distance_to_point2 = m.sqrt((point2[0] - fixed_point[0]) ** 2 + (point2[1] - fixed_point[1]) ** 2)

        if distance_to_point1 >= distance_to_point2:
            return point1
        else:
            return point2

    @staticmethod
    def calculate_vertical_cross_point(line: t_line, point):
        x1, y1, x2, y2 = line
        x, y = point

        # If the line is vertical, the cross point's x-coordinate is the same as the line's x-coordinate
        if x1 == x2:
            cross_point_x = x1
            cross_point_y = y
        else:
            # Calculate the line's slope
            m = (y2 - y1) / (x2 - x1)
            # Calculate the line's y-intercept
            c = y1 - m * x1

            # Calculate the x-coordinate of the vertical cross point
            cross_point_x = x
            # Calculate the y-coordinate using the line equation y = mx + c
            cross_point_y = m * x + c

        return (cross_point_x, cross_point_y)

    def predict_without_target_track_new_03(self, btm_limit: float = 0, top_limit: float = 0):
        """find the best track distance using minimize()"""
        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        bounds = [(btm_limit, top_limit)]
        res = minimize(
            _cal_try_track_error,
            400,
            args=(self.lines, self.view_center_group_cross_line, self.bl_time_tick),
            bounds=bounds,
        )
        print("+" * 55)
        if res.success:
            print(res.fun)
            print(res.x)
        else:
            print("no res.success")

        # self.predict_without_target_track_partial_brute_force(
        #     first_stage_best_distance - 3 * s.Travel_distance_step_first,
        #     first_stage_best_distance + 3 * s.Travel_distance_step_first,
        # )


if __name__ == "__main__":
    print('Do not run this script directly, you will get error "ModuleNotFoundError: No' " module named 'utils'\"")
